'''
毒刺地对空导弹模型
距离单位  百米
速度单位  百米/秒
'''

import numpy as np
import json,math,os,copy

class FIM():
    def __init__(self,name,attach,lable):
        para=json.load(open('Config/EntitiesConfig.json','r',encoding='UTF-8'))
        self.Name='AGM--'+name
        self.Attach=attach  # 隶属于的实体
        self.Lable=lable
        self.Speed=para['FIM_Speed']
        self.Position = []    # 自身坐标
        self.Target=-1
        self.Target_Position = []    # 目标坐标
        self.Master_Position=[] # 母机坐标，根据母机位置变化
        self.PositionMemory=[]
        self.Steps=0    # 记录飞行了的步数
        self.Active=0   # dd的激活状态
        self.StartRound=-1
        self.EndRound=-1

    def Launch(self,target,p_master,p_target,rounds):
        '''
        导弹发射，此时给导弹状态赋初始值
        :param target: 目标编号
        :param p_master: 载机坐标
        :param p_target: 目标坐标
        :return:
        '''
        self.Target=target
        self.Position = p_master.copy  # 赋值值，参数不随外部变化
        self.Target_Position=p_target   # 赋值容器，参数随外部变化
        self.Master_Position=p_master   # 赋值容器，参数随外部变化
        self.Active=1
        self.StartRound=rounds

    def Fly_track(self,dt,rounds):
        dis = self._distance_xyz(self.Position, self.Target_Position)
        copy_pos = self.Position[:]
        self.PositionMemory.append(copy_pos)

        if dis < (self.Speed * dt):
            self.Position = self.Target_Position[:]
            self.Active = 0
            self.EndRound=rounds
        else:
            diff_position = np.array(self.Target_Position) - np.array(self.Position)
            aug_position = diff_position * (self.Speed * dt / dis)
            self.Position = list(aug_position + self.Position)
            self.Steps += 1

    def _distance_xyz(self,A,B):
        x=B[0]-A[0]
        y = B[1] - A[1]
        z = B[2] - A[2]
        dis=math.sqrt(x**2+y**2+z**2)
        return dis